import array

HEADER_SIZE = 8

def getMsgArrayWithHeader():
        data = array.array('B')
        data.append(0xff)
        data.append(0xff)
        data.append(0xaa)
        data.append(0x55)
        data.append(0xaa)
        data.append(0x55)
        data.append(0x37)
        data.append(0xba)
	return data	

def isMessageAndEchoEqual(msg, echo):
	msgSize = len(msg)
	if msgSize != len(echo):
		return False
	
	for a,b in zip(msg, echo):
		if a != b:
			return False
	return True


def validateMessageHeader(msg):
	header = getMsgArrayWithHeader()
	
	if len(msg) < HEADER_SIZE:
		return False

	for a,b in zip(header, msg):
		b = ord(b)
		if a != b:
			return False

	return True

def validateMessageEcho(port, originalMsg):
	size = len(originalMsg)
	rxMsg = port.read(size)
	if validateMessageHeader(rxMsg) == False:
		return False

	originalData = buffer(originalMsg, HEADER_SIZE)
	rxData = buffer(rxMsg, HEADER_SIZE)
	
	if originalData[0] != rxData[0]:
		return False

	# ignore platform field

	originalData = buffer(originalData, 2)
	rxData = buffer(rxData, 2)
	return isMessageAndEchoEqual(originalData, rxData)

def getMotionMessage(motionNum):
	data = getMsgArrayWithHeader()
        data.append(0x14)
        data.append(0x00)
        data.append(0x00)
        data.append(0x00)
        data.append(0x00)
        data.append(0x01)
	data.append(motionNum)
	data.append(motionNum)
	return data

def getDirectControlModeMessage():
	data = getMsgArrayWithHeader()
	data.append(0x10)
	data.append(0x00)
	data.append(0x00)
	data.append(0x00)
	data.append(0x00)
	data.append(0x01)
	data.append(0x01)
	data.append(0x01)
	return data

def getDirectControlModeReleaseMessage():
        data = array.array('B')
	data.append(0xff)
	data.append(0xe0)
	data.append(0xfb)
	data.append(0x01)
	data.append(0x00)
	data.append(0x1a)
	return data

